#include "boxReader.h"

#include "oslib.h"
#include "string.h"
#include "dji2_0.h"
#include <stdlib.h>
#include <math.h>
#include "shooter.h"
#include "lightband.h"

#define MAX_LOCK_COUNT 30
#define MANUAL_SPEED 25

/**
 * -86000 //leftLimit
 * -62500 //取箭点
 */
BoxType boxes[30];

int hasMatched = 0;
int aimingId = 0;
int autoAimingMode = MANUAL;
int running = 0;
static int lockCount = 0;
// PID_Struct aimingPid = {.KP=0.4, .KD=0.18, .KI=0.1, 0, 0, .i_max=3, 0, 0.02};
// PID_Struct aimingPid = {.KP = 0.1, .KD = 0.1, .KI = 0.2, 0, 0, .i_max = 5, 0, .I_TIME=0.05};
// PID_Struct aimingPid = {.KP = 0.3, .KD = 0.1, .KI = 0.3, 0, 0, .i_max = 5, 0, .I_TIME=0.05};
PID_s aimingPid = {.Kp = 0.15, .Kd = 12, .Ki = 0.35, .int_max = 3, .int_duty=0.5, .ctrl_max=20};
// PID_s aimingPid = {.Kp = 0.25, .Kd = 12, .Ki = 0.4, .int_max = 3, .int_duty=2, .ctrl_max=25};


/*
float Kp;
	float Kd;
	float Ki;
	float int_sum;
	float last_err;
	float last_last_err;
	float int_max;
	float last_delta_err;
	float int_duty;
	float ctrl_max;
*/

int manualLeft = 0;         
int manualRight = 0;
int manualArrival = 0;

void setBox(int id, double deltaX, double depth)
{
    boxes[id].deltaX = deltaX;
    boxes[id].depth = depth;
}

void changeManul()
{
    autoAimingMode = MANUAL;
    lb_set_light(3, LB_COLOR_RED, LB_BLINK_KEEP);
    // dji_LimitVelCfg(TRIPOD_HCAN, TRIPOD_BOARD_ID, TRIPOD_MOTOR_ID);
}

void changeAuto()
{
    autoAimingMode = AUTO;
    lb_set_light(3, LB_COLOR_BLUE, LB_BLINK_KEEP);
    // dji_LimitVelCfg(TRIPOD_HCAN, TRIPOD_BOARD_ID, TRIPOD_MOTOR_ID);
}

void manualLeftStart() 
{
    if (autoAimingMode == MANUAL) {
        manualLeft = 1;
        lb_set_light(3, LB_COLOR_PURPLE, LB_BLINK_KEEP);
    }
}

void manualLeftStop() 
{
    manualLeft = 0;
    lb_set_light(3, (autoAimingMode == MANUAL) ? LB_COLOR_RED : LB_COLOR_BLUE, LB_BLINK_KEEP);
}

void manualRightStart()
{
    if (autoAimingMode == MANUAL) {
        manualRight = 1;
        lb_set_light(3, LB_COLOR_PURPLE, LB_BLINK_KEEP);
    }
}

void manualRightStop()
{
    manualRight = 0;
    lb_set_light(3, (autoAimingMode == MANUAL) ? LB_COLOR_RED : LB_COLOR_BLUE, LB_BLINK_KEEP);

}

int manualArrive()
{
    if (running) {
        manualArrival = 1;
        return 0;
    }
    return 1;
}

void updateBoxData(char *buffer, int *count)
{
    char *delim = " ";
    char *token = strtok(buffer, delim);
    int nums = atoi(token);
    // uprintf("nums is %d", nums);
    if (nums >= 30)
    {
        uprintf("nums is error!\r\n");
        return;
    }
    int matchFlag = 0;
    for (int i = 0; i < nums; i++)
    {
        token = strtok(NULL, delim);
        int trackId = atoi(token);

        token = strtok(NULL, delim);
        double deltaX = atof(token);

        token = strtok(NULL, delim);
        double depth = atof(token);
        // static int temp_count = 0;
        // if (temp_count++ % 100) {
        //     uprintf("trackId is %d, deltaX is %.3f, depth is %.3f\r\n", trackId, deltaX, depth);
        // }
        UDebug("trackId is %d, deltaX is %d, depth is %d", trackId, (int)deltaX, (int)depth);
        if (trackId > 30 || trackId < 0)
        {
            uprintf("track Id is %d, out of range!\r\n", trackId);
            return;
        }
        if (trackId == aimingId)
            matchFlag = 1;
        // FIXME 增加读写锁
        setBox(trackId, deltaX, depth);
    }
    hasMatched = matchFlag;
}

/**
 * @brief 处理上位机的桶数据
 * 
 * @param argument None
 */
void boxReaderFunc(void *argument)
{
    for (;;)
    {
        osSemaphoreAcquire(UART2_RxSema, osWaitForever);
        int boxCount = 0;
        updateBoxData(UART2_TaskBuffer, &boxCount);
    }
}

/**
 * @brief 云台控制线程
 * 
 * @param argument None
 */
void tripodHeadFunc(void *argument)
{
    for (;;)
    {
        osSemaphoreAcquire(TripodTokenSema, osWaitForever);
        dji_LimitVelCfg(TRIPOD_HCAN, TRIPOD_BOARD_ID, TRIPOD_MOTOR_ID);
        autoAimingMode = MANUAL;
        running = 1;
        lockCount = 0;
        manualArrival = 0;
        uprintf("Tripod start aiming\n");
        for (;;)
        {
            if (autoAimingMode == AUTO) /* 判断为自动瞄准模式 */
            {
                float output;
                if (hasMatched == 1) //有匹配id
                {
                    output = PID_GetOutput(&aimingPid, 0, boxes[aimingId].deltaX);
                    // uprintf("%.3f\r\n", output);
                }
                else
                {
                    output = 0;
                }
                dji_VelCtrl(TRIPOD_HCAN, TRIPOD_BOARD_ID, TRIPOD_MOTOR_ID, -(int)output);
                if (fabsf(boxes[aimingId].deltaX) <= 3)
                {
                    lockCount++;
                }
                else
                {
                    lockCount = 0;
                }

                if (lockCount > MAX_LOCK_COUNT || manualArrival)
                {
                    uprintf("Tripod auto arrive\r\n");
                    break;
                }
            }
            else if (autoAimingMode == MANUAL)
            {
                if (manualLeft || manualRight)
                {
                    if (manualLeft && manualRight)
                    {
                        dji_VelCtrl(TRIPOD_HCAN, TRIPOD_BOARD_ID, TRIPOD_MOTOR_ID, 0);
                    }
                    else if (manualRight)
                    {
                        dji_VelCtrl(TRIPOD_HCAN, TRIPOD_BOARD_ID, TRIPOD_MOTOR_ID, MANUAL_SPEED);
                    }
                    else
                    {
                        dji_VelCtrl(TRIPOD_HCAN, TRIPOD_BOARD_ID, TRIPOD_MOTOR_ID, -MANUAL_SPEED);
                    }
                }
                else
                {
                    dji_VelCtrl(TRIPOD_HCAN, TRIPOD_BOARD_ID, TRIPOD_MOTOR_ID, 0);
                }
                if (manualArrival) {
                    uprintf("Tripod manual arrive\n");
                    break;
                }
            }
            osDelay(20);
        }
        dji_NoInitPosCfg(TRIPOD_HCAN, TRIPOD_BOARD_ID, TRIPOD_MOTOR_ID, 50);
        autoAimingMode = MANUAL;
        lockCount = 0;
        manualArrival = 0;
        shooter_yaw_ready(shooter1);
        shooter_yaw_arrive(shooter1, boxes[aimingId].depth);
        running = 0;
    }
}
